clear all;

randn('seed',42)
dim = 3;

ps = [848, 238, 0; ...
854, 238, 0; ...
854, 244, 0; ...
854, 256, 0; ...
265, 216.5, -0.0701742; ...
851, 241.5, -0.0701742; ...
857, 241.5, -0.0701742; ...
857, 254.5, -0.0701742; ...
848.5, 245, -0.138028; ...
855.5, 245, -0.138028; ...
855.5, 252, -0.138028; ...
268, 218.5, -0.208784; ...
852, 248.5, -0.208784; ...
859, 248.5, -0.208784; ...
858, 242.5, -0.278359; ...
850, 250.5, -0.278359; ...
858, 250.5, -0.278359; ...
265, 208, -0.346105; ...
570, 335, -0.346105; ...
575.5, 342, -0.416267; ...
];
weights = [2.32922, 3.49596, 2.18197, 2.45493, 1.96251, 3.44777, 1.95501, 1.82764, 2.3261, 2.92285, 2.1724, 2.17116, 3.70239, 2.38294, 1.92256, 2.56496, 3.14652, 2.48983, 2.17828, 2.19807];

N = size(ps,1);


figure(1);
hold off;
plot3(ps(:,1),ps(:,2),ps(:,3),'xb');
hold on;

% test meanShiftEstimate
esigmas = [8 16 1.3];
%weights = normrnd(1,0.5,1,N);
%weights = ones(1,N);
em = meanShiftEstimate(ps,weights,esigmas,0.001,1);

disweights = weights*10;
disweights = max(0.01,disweights);

for i = 1:size(weights,2)
    plot3(ps(i,1),ps(i,2),ps(i,3),'ob','MarkerSize',disweights(1,i),'MarkerEdgeColor',[0.7 0.7 1])
end
plot3(em(:,1),em(:,2),em(:,3),'ro');